﻿#include "../axis-tags/mainwindow.h"
#include <QApplication>

#include <unistd.h>
#include"iostream"
#include<opencv2/opencv.hpp>
#include<thread>
#include<iomanip>
#include<time.h>
#include<fstream>
#include<sstream>
#include"ImgProdCons.h"
#include"./Pose/AngleSolver.hpp"
#include"./Armor/EnemyDetector.h"
#include"./Rune/Rune.h"
#include"./Odometry/Predictor.hpp"

//#define _DEBUG
#define CAMERA_NUMBER 13
//#define GET_VIDEO
#define SHOW_VALUE_PLOT


using namespace std;
using namespace cv;

int self_color;
int ExposureTime = 90;
int Expcount =1000;

namespace rm
{
int Is_tuoluo = 0;//
float pitch_deviation = 0;
float yaw_deviation = 0;
void ImgProdCons::init()
{
#include "../axis-tags/mainwindow.h"
#include <QApplication>
    time_t nowtime;

    //prevent usb port from being blocked
    init_signals();

    time(&nowtime);
    _nowtime = localtime(&nowtime);
    _outfile << "start:  " << _nowtime->tm_mon+1 << "/" << _nowtime->tm_mday << "  " << _nowtime->tm_hour << ":" << _nowtime->tm_min << ":" << _nowtime->tm_sec << endl;

    //Initialize enemy detector
    _enemyDetectorPtr->init();          //load CNN takes lots of time

    //Initialize camera
    _videoCapturePtr->open(0,2);

    _videoCapturePtr->setExposureTime(10);
    _videoCapturePtr->setVideoFormat(1280, 1024, true);
    _videoCapturePtr->setFPS(210.0);
    _videoCapturePtr->setBalanceRatio(1.6, 1.3, 2.0, false);
    _videoCapturePtr->setGain(2);
    _videoCapturePtr->setGamma(1);

    _videoCapturePtr->startStream();
    _videoCapturePtr->closeStream();

    _videoCapturePtr->startStream();

    _videoCapturePtr->info();

    //Initilize serial
    _serialPtr->openPort();
    _serialPtr->setDebug(true);
    int self_color;
    //    while(_serialPtr->setup(self_color) != Serial::OJBK)
    //    {
    //        _outfile<<"hello1"<<endl;
    //        sleep(1);
    //    }
    self_color = rm::BLUE;
    //self_color = rm::RED;
    cout << "I am " << (self_color == rm::BLUE ? "blue" : "red") << "." << endl;

    _enemyDetectorPtr->setEnemyColor(self_color == rm::BLUE ? rm::RED : rm::BLUE);

    //Initialize angle solver
    AngleSolverParam angleParam;
    cout<<"hello 12"<<endl;
    angleParam.readFile(CAMERA_NUMBER);//choose camera
    cout<<"hello 13"<<endl;
    _solverPtr->init(angleParam);
    _solverPtr->setResolution(_videoCapturePtr->getResolution());
    cout<<"hello 14"<<endl;

    //Initialize windmill detector
    _windmillPtr->setColor(self_color);
    _windmillPtr->readFile();

    //Initialize sentry strategies
    //_sentryStrategiesPtr->initPara();
    cout<<"hello 15"<<endl;

    time(&nowtime);
    _nowtime = localtime(&nowtime);
    _outfile << "setup:  " << _nowtime->tm_mon+1 << "/" << _nowtime->tm_mday << "  " << _nowtime->tm_hour << ":" << _nowtime->tm_min << ":" << _nowtime->tm_sec << endl;
    _outfile << "I am " << (self_color == rm::BLUE ? "blue" : "red") << "." << endl;

    _task = Serial::NO_TASK;
}


void ImgProdCons::consume()
{
    int argc;
    char ** argv = nullptr;
    QApplication a(argc, argv);
    cout<<"hello 11"<<endl;

    int pre_sec = -1;

    /*
     * Variables for serials
     */
    ControlData controlData;
#ifdef GET_VIDEO
    /*
     * Variables for recording camera
     */
    VideoWriter writer;
    VideoWriter writer1;
    bool isRecording = true;
    bool readyTOrecord = true;
    time_t t;
    time(&t);
    const string fileName = "/home/nuc/robomaster2022/" + to_string(t) + ".avi";
    const string fileName1 = "/home/nuc/robomaster2022/" + to_string(t) + "raw" + ".avi";
    writer.open(fileName, CV_FOURCC('M', 'J', 'P', 'G'), 25, Size(1280, 1024));
    writer1.open(fileName1, CV_FOURCC('M', 'J', 'P', 'G'), 25, Size(1280, 1024));
#endif
    /*
     *  Variables for angle solve module
     */
    int angleFlag;
    Vec2f targetAngle;

#ifdef SHOW_VALUE_PLOT
    MainWindow w;
    static double* ele_x = new double(0);
    static double* ele_y = new double(0);
    w.addElement(ele_x);
    w.addElement(ele_y);
#endif

    /* Variables for armor detector module */
    int armorType;
    std::vector<cv::Point2f> LightbarVertex;
    /*
     *  The main loop for task
     */
    const double d_ = 0;
    Frame frame;
    for(;;)
    {
        time_t nowtime;
        time(&nowtime);
        _nowtime = localtime(&nowtime);
        if(_nowtime->tm_sec != pre_sec)
        {
            _outfile << "consume:  " << _nowtime->tm_mon+1 << "/" << _nowtime->tm_mday << "  " << _nowtime->tm_hour << ":" << _nowtime->tm_min << ":" << _nowtime->tm_sec << endl;
        }
        pre_sec = _nowtime->tm_sec;
        //_task = Serial::AUTO_SHOOT;
        auto t1 = chrono::high_resolution_clock::now();
#ifndef _MSC_VER
        if (_quit_flag)
            return;
#endif
        if(_serialPtr->getErrorCode() == Serial::SYSTEM_ERROR || !_videoCapturePtr->isOpened())
        {
            this_thread::sleep_for(chrono::seconds(3));
        }

        if(!_buffer.getLatest(frame))
        {
            //_outfile<<"get Latest frame failed"<<endl;
            //cout<<"get Latest frame failed"<<endl;
            continue;

        }
        Expcount++;
        if(Expcount>=300)
        {
            Mat tmp = frame.img;
            cvtColor(frame.img,tmp,COLOR_BGR2GRAY);
            int T=0; //Otsu算法阈值
            double varValue=0; //类间方差中间值保存
            double w0=0; //前景像素点数所占比例
            double w1=0; //背景像素点数所占比例
            double u0=0; //前景平均灰度
            double u1=0; //背景平均灰度
            double Histogram[256]={0}; //灰度直方图，下标是灰度值，保存内容是灰度值对应的像素点总数
            uchar *data=tmp.data;
            double totalNum=tmp.rows*tmp.cols; //像素总数
            //计算灰度直方图分布，Histogram数组下标是灰度值，保存内容是灰度值对应像素点数
            for(int i=0;i<tmp.rows;i++)   //为表述清晰，并没有把rows和cols单独提出来
            {
                for(int j=0;j<tmp.cols;j++)
                {
                    Histogram[data[i*tmp.step+j]]++;
                }
            }
            for(int i=0;i<255;i++)
            {
                //每次遍历之前初始化各变量
                w1=0;		u1=0;		w0=0;		u0=0;
                //***********背景各分量值计算**************************
                for(int j=0;j<=i;j++) //背景部分各值计算
                {
                    w1+=Histogram[j];  //背景部分像素点总数
                    u1+=j*Histogram[j]; //背景部分像素总灰度和
                }
                if(w1==0) //背景部分像素点数为0时退出
                {
                    break;
                }
                u1=u1/w1; //背景像素平均灰度
                w1=w1/totalNum; // 背景部分像素点数所占比例
                //***********背景各分量值计算**************************

                //***********前景各分量值计算**************************
                for(int k=i+1;k<255;k++)
                {
                    w0+=Histogram[k];  //前景部分像素点总数
                    u0+=k*Histogram[k]; //前景部分像素总灰度和
                }
                if(w0==0) //前景部分像素点数为0时退出
                {
                    break;
                }
                u0=u0/w0; //前景像素平均灰度
                w0=w0/totalNum; // 前景部分像素点数所占比例
                //***********前景各分量值计算**************************

                //***********类间方差计算******************************
                double varValueI=w0*w1*(u1-u0)*(u1-u0); //当前类间方差计算
                if(varValue<varValueI)
                {
                    varValue=varValueI;
                    T=i;
                }
            }
            if(T>=150)
                ExposureTime=1;
            else if(T>=100)
                ExposureTime=2;
            else if(T>=50)
                ExposureTime=3;
            else if(T>=30)
                ExposureTime=4;
            else
                ExposureTime=5;
            Expcount=0;
            //            ExposureFlag=1;
            cout<<"\nExposure Changed to"<<ExposureTime<<endl;
            //            Correct Exposure
            _videoCapturePtr->closeStream();
            _videoCapturePtr->setExposureTime(ExposureTime);
            _videoCapturePtr->startStream();
            _videoCapturePtr->info();
        }

        if(_task == Serial::BUFF)
            //if(1)
        {
            cout<<endl;
            chrono::high_resolution_clock::time_point t1, t2;

            t1 = chrono::high_resolution_clock::now();

            //==============================================
            _videoCapturePtr->setExposureTime(4);
            //==============================================

            _windmillPtr->loadImg(frame.img, frame.timeStamp);
            _windmillPtr->Detect();

            Point2f windmillArmor;
            if(_windmillPtr->getIsfund())
            {
                windmillArmor =  _windmillPtr->getNextPoint(); //指向预测点
                LightbarVertex = _windmillPtr->getArmorInsideVertex();
                _solverPtr->setTarget(LightbarVertex, windmillArmor, 2);//利用单点结算来指向
                angleFlag = _solverPtr->solve();
                if(angleFlag != rm::AngleSolver::ANGLE_ERROR)
                {
                    targetAngle = _solverPtr->getAngle();

                    controlData.frame_seq   = frame.seq;
                    //                    controlData.shoot_mode  = Serial::SINGLE_FIRE | Serial::HIGH_SPEED;


                    //=============================== gravity offset==/

                    double d = 7300;//horizontal distance
                    double angleNow=frame.poseEuler.gimbal_pitch-targetAngle[1];
                    const double d_ = _solverPtr->getDistance();
                    cout<<"dis"<<d_<<endl;
                    if(abs(acos(_solverPtr->getDirection().x)*180/CV_PI - 90) < 5)
                    {
                        d=d_;
                    }
                    double pnpDistance=d/cos(angleNow*(CV_PI / 180));//7.49

                    //<<aangleNow<<"____anglenow"<<endl;

                    double deltaAngle = gravityOffset(bullet_speed, angleNow, pnpDistance) ;
                    //========================================
                    if(abs(targetAngle[0])<1.5)
                    {
                        _windmillPtr ->_loopNumber++;
                        _windmillPtr ->_loopNumber = _windmillPtr ->_loopNumber%16;
                        //<<"sheshe(((((((((((((((((((((((()))))))))))))))))))))"<<_windmillPtr ->_loopNumber<<endl;
                    }
                    else
                        _windmillPtr ->_loopNumber = 0;

                    if(windmillArmor.x != 0 && windmillArmor.y != 0)
                    {
                        controlData.pitch_dev   = -targetAngle[1] + deltaAngle - 0.6;
                        controlData.yaw_dev     = -targetAngle[0] + 0.2;//right-  left+
                        controlData.gimbal_mode = Serial::ARMOR_FOUND;//target fund
                    }
                    else
                    {
                        controlData.pitch_dev   = 0;
                        controlData.yaw_dev     = 0;
                        controlData.gimbal_mode =   Serial::ARMOR_NOT_FOUND;//target not fund

                    }
                    //change the threshold of the shooting condition
                    if(_windmillPtr->_loopNumber >= 2 && _windmillPtr->_loopNumber <= 5/*&& abs(controlData.yaw_dev)<0.4 */)//_windmillPtr->isHit()&&_windmillPtr ->_loopNumber == 3)
                    {
                        controlData.shoot_mode  = Serial::BURST_FIRE | Serial::HIGH_SPEED;
                    }
                    else
                    {
                        controlData.shoot_mode  = Serial::NO_FIRE | Serial::HIGH_SPEED;
                    }


#ifdef SENTRY
                    controlData.gimbal_mode = Serial::SERVO_MODE;
#endif
                    int error = _serialPtr->tryControl(controlData, chrono::milliseconds(5));

                    if(error != Serial::OJBK)
                    {
                        _outfile<<"hit buff trycontrol failed:    "<<dec<<error<<endl;
                    }
                    //cout<< "windmill Deviation: " << "["<<controlData.pitch_dev << " " << controlData.yaw_dev<<"]" << endl;

                }
            }
            //            else
            //            {
            //                cout<<1<<endl;
            //            }
            t2 = chrono::high_resolution_clock::now();
            cout << "Detection duration: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;

        }
        //        if(_task==Serial::AUTO_SHOOT)
        if(1)
        {
            chrono::high_resolution_clock::time_point t1, t2;

            t1 = chrono::high_resolution_clock::now();

            //==============================================
            _videoCapturePtr->setExposureTime(8);
            //==============================================
            //            imshow("pppp",_newImg);
            //            waitKey(1);
            _enemyDetectorPtr->loadImg(/*_newImg*/frame.img);
            if(_enemyDetectorPtr->detect())
            {
                LightbarVertex = _enemyDetectorPtr->getTargetVertex();
                armorType = _enemyDetectorPtr->getArmorType();

                cv::Point2f t_point=_enemyDetectorPtr->getTargetCenter();
                _solverPtr->setTarget(LightbarVertex,t_point, armorType);


                angleFlag = _solverPtr->solve();
                if(angleFlag != rm::AngleSolver::ANGLE_ERROR)
                {
                    targetAngle = _solverPtr->getAngle();
                    //=============================== gravity offset==/
                    cout<<"pitch:    "<<frame.poseEuler.gimbal_pitch<<"     yaw:     "<<frame.poseEuler.gimbal_yaw<<endl;
                    //double pnpDistance=_solverPtr->KF_Distance();
                    double pnpDistance=_solverPtr->getDistance();
                    cout<<pnpDistance<<endl;
                    double h = 0.03;
                    double angleNow = -frame.poseEuler.gimbal_pitch-targetAngle[1]-(180/CV_PI)*(atan(h/2)-atan(h*1000/pnpDistance));//yaw is pitch;dianji he laobubing buzai tongyibian, suoyi you fuhao


                    cout<<"dis:   "<<pnpDistance<<endl;
                    cout<<"bullet_speed:     "<<bullet_speed<<"..........."<<endl;

                    //double deltaAngle = gravityOffset(27.8, angleNow, pnpDistance);
                    double deltaAngle = gravityOffset(bullet_speed, angleNow, pnpDistance);
                    cout<<"deltaAngle"<<deltaAngle<<endl;
                    //========================================
                    controlData.frame_seq   = frame.seq;
                    controlData.shoot_mode  = Serial::BURST_FIRE | Serial::LOW_SPEED;
                    ////controlData.shoot_mode=Serial::ControlMode::REQUEST_TRANS;

                    Point3f n = _solverPtr->getDirection();
                    _predictorPtr->push(frame.timeStamp, frame.poseEuler.gimbal_pitch, frame.poseEuler.gimbal_yaw, _enemyDetectorPtr->getArmorNum(), pnpDistance, targetAngle, n);
                    _predictorPtr->setBulletSpeed(bullet_speed);
                    _predictorPtr->predict();
                    //_predictorPtr->print();
                    Vec2f compensate =  _predictorPtr->getCompensate();

#ifdef SHOW_VALUE_PLOT
                    *ele_x = pnpDistance;
                    //*ele_y = enemydetector.getTargetCenter().y;
                    w.plot();
                    w.show();
#endif

                    ostringstream oss;
                    oss<< "["<<compensate[0] << " " << compensate[1]<<"]  "<<_predictorPtr->v_t;
                    string str(oss.str());
                    putText(frame.img, str, Point(25,25), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,195,0), 1);

                    if(Is_tuoluo)
                    {
                        controlData.pitch_dev   =  - targetAngle[1] + pitch_deviation + deltaAngle;
                        controlData.yaw_dev     =  - targetAngle[0] + yaw_deviation;
                        cout << "Deviation: " << "["<<targetAngle[1]<<" " << targetAngle[0]<<"]" << endl;
                    }

                    if(CAMERA_NUMBER == 13 )
                    {
                        controlData.pitch_dev   =   - targetAngle[1] + deltaAngle - 0.6;//  + compensate[0];
                        controlData.yaw_dev     =   - targetAngle[0] + 0.2;// - compensate[1];// + 0.5; + left
                    }
                    controlData.gimbal_mode = Serial::ARMOR_FOUND;//target fund
                    int error = _serialPtr->tryControl(controlData, chrono::milliseconds(1));
                    if(error != Serial::OJBK)
                    {
                        cout<<dec<<error<<endl;
                        _outfile << "autoshoot tryControl failed:    " << dec << error << endl;
                        cout<<"not sent"<<endl;
                    }
                    cout << "Deviation: " << "["<<controlData.pitch_dev << " " << controlData.yaw_dev<<"]" << endl;
                }
            }
            else
            {
                controlData.pitch_dev   =   0;
                controlData.yaw_dev     =   0;
                controlData.gimbal_mode =   Serial::ARMOR_NOT_FOUND;//target not fund

                int error = _serialPtr->tryControl(controlData, chrono::milliseconds(5));
                if(error != Serial::OJBK)
                {
                    cout<<dec<<error<<endl;
                    _outfile << "autoshoot tryControl failed:    " << dec << error << endl;
                    cout<<"not sent"<<endl;
                }
                //cout << "Deviation: " << "[0, 0]" << endl;
            }
            // cout<<"dis"<<_solverPtr->getDistance()<<endl;
            //this_thread::sleep_for(chrono::milliseconds(2));
            t2 = chrono::high_resolution_clock::now();
            cout << "Detection duration: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;

        }


#ifdef GET_VIDEO
        if(isRecording)
        {
            if(_task == Serial::BUFF)
            {
                writer << _windmillPtr->getImage();
            }

            if(_task == Serial::AUTO_SHOOT)
            {
                Mat img = _enemyDetectorPtr->getDebugImg();

                ostringstream oss;
                oss<< "["<<controlData.pitch_dev << " " << controlData.yaw_dev<<"]";
                string str(oss.str());

                putText(img, str, Point(25,25), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,195,0), 1);
                writer << img;
            }
            writer1 << frame.img;
        }
        if(!writer.isOpened())
        {
            //cout<<"capture failed"<<endl;
            continue;
        }

        if(!writer1.isOpened())
        {
            //cout<<"capture failed"<<endl;
            continue;
        }

        isRecording = true;
#endif
#ifdef _DEBUG
        if(waitKey(1) == 'q')
        {
            return;
        }

#endif

    }
}
}
